// --------------------------------------------------------------------------------------------------------------------
// <copyright file="KinectV2Processor.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Vision.Cameras.KinectV2
{
    using System;
    using System.Drawing;
    using System.IO;
    using System.Runtime.Serialization;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Vision.Cameras;

    /// <summary>
    /// Kinect message processor for kinect V2. 
    /// </summary>
    public class KinectV2Processor
    {
        /// <summary>
        /// Target number of frames to average before publishing
        /// </summary>
        private int averagingCount = 10;

        /// <summary>
        /// Averaged Depth Frame
        /// </summary>
        private ImageFrameDepth averageDepthFrame;

        /// <summary>
        /// Number of frames accumulated into average
        /// </summary>
        private short depthFrameCount;

        /// <summary>
        /// Per pixel count of depth frames averaged
        /// </summary>
        private short[] depthCountPerPixel;

        /// <summary>
        /// Averaged IR frame
        /// </summary>
        private ImageFrame<ushort> averageIRFrame;

        /// <summary>
        /// Number of IR frames averaged
        /// </summary>
        private ushort irFrameCount;

        /// <summary>
        /// Initializes a new instance of the <see cref="KinectV2Processor"/> class. 
        /// </summary>
        /// <param name="averagingCount">Number of raw frames to average per publish</param>
        public KinectV2Processor(int averagingCount)
        {
            this.averagingCount = averagingCount;
        }

        /// <summary>
        /// Gets a value indicating whether sufficient frames have accumulated in the averaging buffer
        /// for either the depth or IR frame.
        /// </summary>
        public bool IsAverageReady
        {
            get
            {
                return this.depthFrameCount >= this.averagingCount || this.irFrameCount >= this.averagingCount;
            }
        }

        /// <summary>
        /// Gets the time of the first message at start of accumulation period
        /// </summary>
        public long AccumulateStartTime { get; private set; }

        /// <summary>
        /// Adds a depth frame to running depth average. 
        /// Skips invalid pixels in the average
        /// </summary>
        /// <param name="depthFrame">Depth frame to add</param>
        /// <param name="minimumDepth">Minimum valid depth</param>
        /// <param name="maximumDepth">Maximum valid depth</param>
        /// <param name="originateTime">Originating of the message for this frame</param>
        /// <returns>True if average if average is ready for reporting</returns>
        public bool AccumulateDepthFrame(ImageFrameDepth depthFrame, short minimumDepth, short maximumDepth, long originateTime)
        {
            this.SetAccumulateStartTime(originateTime);

            int len = depthFrame.Width * depthFrame.Height;
            if (null == this.averageDepthFrame)
            {
                this.averageDepthFrame = new ImageFrameDepth(depthFrame.Width, depthFrame.Height);
                this.depthCountPerPixel = new short[len];
                this.depthFrameCount = 0;
            }

            for (int i = 0; i < len; ++i)
            {
                short v = depthFrame[i];
                if (v >= minimumDepth && v <= maximumDepth)
                {
                    short prevCount = this.depthCountPerPixel[i];
                    ++this.depthCountPerPixel[i];
                    this.averageDepthFrame[i] = (short)((prevCount * this.averageDepthFrame[i] + v) / this.depthCountPerPixel[i]);
                }
            }

            ++this.depthFrameCount;

            return this.IsAverageReady;
        }

        /// <summary>
        /// Get a copy of the current averaged depth frame
        /// </summary>
        /// <returns>Deep copy of the averaged depth frame</returns>
        public ImageFrameDepth GetDepthFrameCopy()
        {
            if (null != this.averageDepthFrame)
            {
                return new ImageFrameDepth(this.averageDepthFrame.Clone());
            }

            return null;
        }

        /// <summary>
        /// Add current depth message to running depth average
        /// </summary>
        /// <param name="irFrame">IR frame to average</param>
        /// <param name="originateTime">Originating of the message for this frame</param>
        /// <returns>True if average if average is ready for reporting</returns>
        public bool AccumulateIRFrame(ImageFrame<ushort> irFrame, long originateTime)
        {
            this.SetAccumulateStartTime(originateTime);

            int len = irFrame.Width * irFrame.Height;
            if (null == this.averageIRFrame)
            {
                this.averageIRFrame = new ImageFrame<ushort>(irFrame.Width, irFrame.Height);
                this.irFrameCount = 0;
            }

            ushort prevCount = this.irFrameCount;
            ++this.irFrameCount;

            for (int i = 0; i < len; ++i)
            {
                this.averageIRFrame[i] = (ushort)((prevCount * this.averageIRFrame[i] + irFrame[i]) / this.irFrameCount);
            }

            return this.IsAverageReady;
        }

        /// <summary>
        /// Get a copy of the current averaged IR frame
        /// </summary>
        /// <returns>Deep copy of the averaged IR frame</returns>
        public ImageFrame<ushort> GetIRFrameCopy()
        {
            if (null != this.averageIRFrame)
            {
                return this.averageIRFrame.Clone();
            }

            return null;
        }

        /// <summary>
        /// Reset the averaging accumulators
        /// </summary>
        /// <remarks>Caller is responsible for ensuring averaging accumulators have been initialized before calling</remarks>
        public void ResetAveragingAccumulators()
        {
            this.depthFrameCount = 0;
            this.irFrameCount = 0;

            if (null != this.averageDepthFrame)
            {
                Array.Clear(this.averageDepthFrame.ImageData, 0, this.averageDepthFrame.ImageData.Length);
            }

            if (null != this.averageIRFrame)
            {
                Array.Clear(this.averageIRFrame.ImageData, 0, this.averageIRFrame.ImageData.Length);
            }

            if (null != this.depthCountPerPixel)
            {
                Array.Clear(this.depthCountPerPixel, 0, this.depthCountPerPixel.Length);
            }
        }

        /// <summary>
        /// Sets the start time of accumulation as time stamp of
        /// the first message received at start of accumulation
        /// </summary>
        /// <param name="time">Received agent message time</param>
        private void SetAccumulateStartTime(long time)
        {
            if (0 == this.depthFrameCount && 0 == this.irFrameCount)
            {
                this.AccumulateStartTime = time;
            }
        }
    }
}
